#ifndef _matrix_ops_hpp_
#define _matrix_ops_hpp_

namespace core {

    namespace math {

        template<class T>
            matrix<T,3> inverse(const matrix<T,3>& m)
            {
                T m00 = (m[1][1]*m[2][2]-m[2][1]*m[1][2]),
                  m10 = (m[0][1]*m[2][2]-m[2][1]*m[0][2]),
                  m20 = (m[0][1]*m[1][2]-m[1][1]*m[0][2]);

                T det =   m[0][0]*m00
                        - m[1][0]*m10
                        + m[2][0]*m20;

                matrix<T,3> r(m00/det,(-1.0)*m10/det, m20/det,
                                (-1.0)*(m[1][0]*m[2][2]-m[2][0]*m[1][2])/det, (m[0][0]*m[2][2]-m[2][0]*m[0][2])/det, (-1.0)*(m[0][0]*m[1][2]-m[1][0]*m[0][2])/det,
                                (m[1][0]*m[2][1]-m[2][0]*m[1][1])/det, (-1.0)*(m[0][0]*m[2][1]-m[2][0]*m[0][1])/det, (m[0][0]*m[1][1]-m[1][0]*m[0][1])/det);
                return r;
            }


        template<class T>
            matrix<T,2> inverse(const matrix<T,2>& m)
            {
                T det =   m[0][0]*m[1][1] - m[1][0]*m[0][1];

                matrix<T,2> r(  m[1][1]/det, -m[0][1]/det,
                                -m[1][0]/det, m[0][0]/det);
                return r;
            }

        //fast inversion for homogeneous 4x4 matrices representing only rotations and translations
        template<class T>
            matrix<T,4>    hom_inverse(const matrix<T,4>& m)
            {
                using std::swap;
                matrix<T,4> temp(m);
                swap(temp[0][1],temp[1][0]);
                swap(temp[0][2],temp[2][0]);
                swap(temp[1][2],temp[2][1]);

                T   t1  =   temp[0][0]*temp[0][3] + temp[0][1]*temp[1][3] + temp[0][2]*temp[2][3],
                    t2  =   temp[1][0]*temp[0][3] + temp[1][1]*temp[1][3] + temp[1][2]*temp[2][3],
                    t3  =   temp[2][0]*temp[0][3] + temp[2][1]*temp[1][3] + temp[3][2]*temp[2][3];
                temp[0][3]  =   -t1;
                temp[1][3]  =   -t2;
                temp[2][3]  =   -t3;
                temp[3][0]  =   0;
                temp[3][1]  =   0;
                temp[3][2]  =   0;
                temp[3][3]  =   1;
            }

        template<class T, int D>
            matrix<T,D> transpose(const matrix<T,D>& m)
            {
                matrix<T,D> r;
                for(int i=0;i<D;++i)
                    for(int j=0;j<D;++j)
                        r[i][j] = m[j][i];
                return r;
            }

        //fast inversion for orthogonal matrices
        template<class T, int D>
            matrix<T,D> ortho_inverse(const matrix<T,D>& m)
            {
                return transpose(m);
            }

        template<class T, int D>
            matrix<T,D> operator*(const matrix<T,D>& a, const matrix<T,D>& b)
            {
                matrix<T,D> c(a);
                c *= b;
                return c;
            }
    } // namespace math

} // namespace core
#endif // _matrix_ops_hpp_
